// --------------------------------------------------------------------------------------------------------------------
// <copyright file="KinectAgentRawMessage.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Vision.Runtime.Cameras
{
    using System;
    using System.Drawing;
    using System.Runtime.Serialization;
    using Microsoft.Kinect;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;
    using Microsoft.Robotics.Vision.Cameras;

    /// <summary>
    /// The KinectAgentRawMessage class represents "raw" depth data from the Kinect
    /// In most cases agents should not consume this message, but consume KinectAgentMessage which is
    /// a processed version of the raw message
    /// Remove this class after all recordings have been converted: Task 2222
    /// </summary>
    [DataContract]
    public class KinectAgentRawMessage : AgentMessage
    {
        /// <summary>
        /// Gets the state of the kinect camera
        /// </summary>
        [DataMember]
        public readonly KinectCameraState State;

        /// <summary>
        /// Gets the intrinsic matrix for camera that produced the message
        /// </summary>
        [DataMember]
        public readonly Matrix3 IntrinsicMatrix;

        /// <summary>
        /// Gets the RGB image frame
        /// </summary>
        [DataMember]
        public readonly ImageFrameRgb RgbFrame;

        /// <summary>
        /// Gets the IR image frame
        /// </summary>
        [DataMember]
        public readonly ImageFrame<ushort> IrFrame;

        /// <summary>
        /// Initializes a new instance of the <see cref="KinectAgentRawMessage" /> class
        /// </summary>
        /// <param name="originatingTime">The depth image creation time.</param>
        /// <param name="depthImage">The depth image</param>
        /// <param name="rgbImage">The RGB image</param>
        /// <param name="depthToRgb">Mapping from the depth image to the RGB image</param>
        /// <param name="rgbToDepth">Mapping from the RGB image to the depth image</param>
        /// <param name="depthImageSize">Size of the depth image</param>
        /// <param name="fieldOfView">The camera horizontal field of view</param>
        /// <param name="maximumRange">Maximum depth range</param>
        /// <param name="minimumRange">Minimum depth range</param>
        /// <param name="furtherThanMaxValue">Reserved value for pixels further than max value (typically negative)</param>
        /// <param name="nearerThanMinValue">Reserved value for pixels closer than min value (typically negative)</param>
        /// <param name="noReadingValue">Reserved value for pixels that have no reading (typically negative)</param>
        /// <param name="pose">Current robot-relative pose.</param>
        /// <param name="state">Camera state</param>
        /// <param name="depthFrame">Depth image frame</param>
        /// <param name="rgbFrame">RGB camera frame. Only one of RGB or IR frame can be non null</param>
        /// <param name="irFrame">IR camera frame. Only one of RGB or IR frame can be non null</param>
        /// <param name="intrinsicMatrix">Camera intrinsic matrix</param>
        public KinectAgentRawMessage(
            long originatingTime,
            short[] depthImage,
            byte[] rgbImage,
            ImageFrame<ColorImagePoint> depthToRgb,
            ImageFrame<DepthImagePoint> rgbToDepth,
            Size depthImageSize,
            double fieldOfView,
            short maximumRange,
            short minimumRange,
            short furtherThanMaxValue,
            short nearerThanMinValue,
            short noReadingValue,
            Pose pose,
            KinectCameraState state,
            ImageFrameDepth depthFrame,
            ImageFrameRgb rgbFrame,
            ImageFrame<ushort> irFrame,
            Matrix3 intrinsicMatrix = null)
            : base(originatingTime)
        {
            short maxReservedValue = 0;
            this.DepthImage = depthImage;
            this.RGBImage = rgbImage;
            this.DepthToRgb = depthToRgb;
            this.RgbToDepth = rgbToDepth;
            this.DepthImageSize = depthImageSize;
            this.FieldOfView = fieldOfView;
            this.MaximumRange = maximumRange;
            this.MinimumRange = minimumRange;
            this.FurtherThanMaxDepthValue = furtherThanMaxValue;
            maxReservedValue = furtherThanMaxValue;
            this.NearerThanMinDepthValue = nearerThanMinValue;
            maxReservedValue = Math.Max(nearerThanMinValue, maxReservedValue);
            this.NoReadingValue = noReadingValue;
            maxReservedValue = Math.Max(noReadingValue, maxReservedValue);
            this.MaximumReservedValue = maxReservedValue;
            this.Pose = pose;                
            this.State = state;
            this.DepthFrame = depthFrame;
            this.RgbFrame = rgbFrame;
            this.IrFrame = irFrame;
            this.IntrinsicMatrix = intrinsicMatrix;
        }

        /// <summary>
        /// Gets the depth array  (Task 1892 Legacy will be deleted)
        /// </summary>
        [DataMember]
        public short[] DepthImage { get; private set; }

        /// <summary>
        /// Gets the depth image size (Task 1892 Legacy will be deleted)
        /// </summary>
        [DataMember]
        public Size DepthImageSize { get; private set; }

        /// <summary>
        /// Gets the mapping from the depth image to the RGB image
        /// </summary>
        [DataMember]
        public ImageFrame<ColorImagePoint> DepthToRgb { get; private set; }

        /// <summary>
        /// Gets the horizontal field of view, in radians
        /// </summary>
        [DataMember]
        public double FieldOfView { get; private set; }

        /// <summary>
        /// Gets the max range in millimeters
        /// </summary>
        [DataMember]
        public short MaximumRange { get; private set; }

        /// <summary>
        /// Gets the min range in millimeters
        /// </summary>
        [DataMember]
        public short MinimumRange { get; private set; }

        /// <summary>
        /// Gets the depth sample value returned by Kinect when 
        /// the range is further than the max value
        /// </summary>
        [DataMember]
        public short FurtherThanMaxDepthValue { get; private set; }

        /// <summary>
        /// Gets the depth sample value returned by Kinect when 
        /// the range is nearer than the min value
        /// </summary>
        [DataMember]
        public short NearerThanMinDepthValue { get; private set; }

        /// <summary>
        /// Gets the depth sample value returned for
        /// no reading.
        /// </summary>
        [DataMember]
        public short NoReadingValue { get; private set; }

        /// <summary>
        /// Gets the maximum of the reserved values
        /// </summary>
        [DataMember]
        public short MaximumReservedValue { get; private set; }

        /// <summary>
        /// Gets or sets pose of the Kinect.
        /// </summary>
        [DataMember]
        public Pose Pose { get; set; }

        /// <summary>
        /// Gets or sets the panning speed, in rad/s. Useful in applying rolling shutter corrections.
        /// </summary>
        [DataMember]
        public double PanningSpeed { get; set; }

        /// <summary>
        /// Gets the RGB array, a regular RGB array - 3 bytes per pixel. If RGB collection is not enabled
        /// this array is of zero size. When both depth and color are enabled and aligned then the depth 
        /// and RGB arrays have the same number of pixels
        /// (Legacy will be deleted)
        /// </summary>
        [DataMember]
        public byte[] RGBImage { get; private set; }

        /// <summary>
        /// Gets the mapping from the RGB image to the depth image
        /// </summary>
        [DataMember]
        public ImageFrame<DepthImagePoint> RgbToDepth { get; private set; }

        /// <summary>
        /// Gets the Depth image frame
        /// </summary>
        [DataMember]
        public ImageFrameDepth DepthFrame { get; private set; }

        /// <summary>
        /// Post-deserialization callback.
        /// </summary>
        /// <param name="context">Deserialization context (unused).</param>
        [OnDeserialized]
        private void OnDeserialized(StreamingContext context)
        {
            if (this.Pose == default(Pose))
            {
                // Patch old recording (assume Max hardware)
                this.Pose = new Pose(new Vector3(0.059, 0, 0.539), new Quaternion(0, 0, 0, 1));
            }

            // This is a temporary approach to allowing recordings that do not
            // have ImageFrame* to continue to be used
            if (null == this.DepthFrame &&
                this.DepthImage != null &&
                this.DepthImage.Length == this.DepthImageSize.Width * this.DepthImageSize.Height)
            {
                this.DepthFrame = new ImageFrameDepth(this.DepthImageSize.Width, this.DepthImageSize.Height);
                for (int i = 0; i < this.DepthImage.Length; ++i)
                {
                    this.DepthFrame.ImageData[i] = this.DepthImage[i];
                }
            }
        }
    }
}
